
/***************************** Include Files *********************************/
#include "../driver/xspi.h"		/* SPI device driver */
#include "flash.h"
#include <string.h>
/************************** Constant Definitions *****************************/

/*
 * The following constants map to the XPAR parameters created in the
 * xparameters.h file. They are defined here such that a user can easily
 * change all the needed parameters in one place.
 */
#define SPI_DEVICE_ID			0

/*
 * The following constant defines the slave select signal that is used to
 * to select the Flash device on the SPI bus, this signal is typically
 * connected to the chip select of the device.
 */
#define SPI_SELECT 			0x01


#define SPI_CMD_RESET_ENABLE                 0x66
#define SPI_CMD_RESET_MEMORY                 0x99
#define SPI_CMD_READ_ID                      0x9F
#define SPI_CMD_READ                         0x03
#define SPI_CMD_FAST_READ                    0x0B
#define SPI_CMD_FAST_READ_DUAL_OUT           0x3B
#define SPI_CMD_FAST_READ_DUAL_IO            0xBB
#define SPI_CMD_FAST_READ_QUAD_OUT           0x6B
#define SPI_CMD_FAST_READ_QUAD_IO            0xEB
#define SPI_CMD_DTR_FAST_READ                0x0D
#define SPI_CMD_DTR_FAST_READ_DUAL_OUT       0x3D
#define SPI_CMD_DTR_FAST_READ_DUAL_IO        0xBD
#define SPI_CMD_DTR_FAST_READ_QUAD_OUT       0x6D
#define SPI_CMD_DTR_FAST_READ_QUAD_IO        0xED
#define SPI_CMD_4B_READ                      0x13
#define SPI_CMD_4B_FAST_READ                 0x0C
#define SPI_CMD_4B_FAST_READ_DUAL_OUT        0x3C
#define SPI_CMD_4B_FAST_READ_DUAL_IO         0xBC
#define SPI_CMD_4B_FAST_READ_QUAD_OUT        0x6C
#define SPI_CMD_4B_FAST_READ_QUAD_IO         0xEC
#define SPI_CMD_4B_DTR_FAST_READ             0x0E
#define SPI_CMD_4B_DTR_FAST_READ_DUAL_IO     0xBE
#define SPI_CMD_4B_DTR_FAST_READ_QUAD_IO     0xEE
#define SPI_CMD_WRITE_ENABLE                 0x06
#define SPI_CMD_WRITE_DISABLE                0x04
#define SPI_CMD_READ_STATUS_REG              0x05
#define SPI_CMD_READ_FLAG_STATUS_REG         0x70
#define SPI_CMD_READ_NV_CONFIG_REG           0xB5
#define SPI_CMD_READ_V_CONFIG_REG            0x85
#define SPI_CMD_READ_EV_CONFIG_REG           0x65
#define SPI_CMD_READ_EXT_ADDR_REG            0xC8
#define SPI_CMD_WRITE_STATUS_REG             0x01
#define SPI_CMD_WRITE_NV_CONFIG_REG          0xB1
#define SPI_CMD_WRITE_V_CONFIG_REG           0x81
#define SPI_CMD_WRITE_EV_CONFIG_REG          0x61
#define SPI_CMD_WRITE_EXT_ADDR_REG           0xC5
#define SPI_CMD_CLEAR_FLAG_STATUS_REG        0x50
#define SPI_CMD_PAGE_PROGRAM                 0x02
#define SPI_CMD_PAGE_PROGRAM_DUAL_IN         0xA2
#define SPI_CMD_PAGE_PROGRAM_DUAL_IN_EXT     0xD2
#define SPI_CMD_PAGE_PROGRAM_QUAD_IN         0x32
#define SPI_CMD_PAGE_PROGRAM_QUAD_IN_EXT     0x38
#define SPI_CMD_4B_PAGE_PROGRAM              0x12
#define SPI_CMD_4B_PAGE_PROGRAM_QUAD_IN      0x34
#define SPI_CMD_4B_PAGE_PROGRAM_QUAD_IN_EXT  0x3E
#define SPI_CMD_32KB_SUBSECTOR_ERASE         0x52
#define SPI_CMD_4KB_SUBSECTOR_ERASE          0x20
#define SPI_CMD_SECTOR_ERASE                 0xD8
#define SPI_CMD_BULK_ERASE                   0xC7
#define SPI_CMD_4B_4KB_SUBSECTOR_ERASE       0x21
#define SPI_CMD_4B_SECTOR_ERASE              0xDC
#define SPI_CMD_PROGRAM_SUSPEND              0x75
#define SPI_CMD_PROGRAM_RESUME               0x7A
#define SPI_CMD_READ_OTP_ARRAY               0x4B
#define SPI_CMD_PROGRAM_OTP_ARRAY            0x42
#define SPI_CMD_ENTER_4B_ADDR_MODE           0xB7
#define SPI_CMD_EXIT_4B_ADDR_MODE            0xE9
#define SPI_CMD_ENTER_QUAD_IO_MODE           0x35
#define SPI_CMD_EXIT_QUAD_IO_MODE            0xF5
#define SPI_CMD_ENTER_DEEP_POWER_DOWN        0xB9
#define SPI_CMD_EXIT_DEEP_POWER_DOWN         0xAB

#define PAGE_SIZE       0x100
#define SUBSECTOR_SIZE  0x1000
#define SECTOR_SIZE     0x10000
/*
 * Definitions of the commands shown in this example.
 */
#define COMMAND_PAGE_PROGRAM		0x02 /* Page Program command */
#define COMMAND_QUAD_WRITE			0x32 /* Quad Input Fast Program */

#define COMMAND_READ			0x03 /* Random read command */
#define COMMAND_FAST_READ		0x0b

#define COMMAND_DUAL_READ			0x3C /* Dual Output Fast Read 4-BYTE DUAL OUTPUT FAST READ 0x3c*/
#define COMMAND_DUAL_IO_READ		0xBC /* Dual IO Fast Read 0xbc*/
#define COMMAND_QUAD_4BYTE_READ		0xEC /* Quad IO Fast Read 0xec*/
#define COMMAND_QUAD_FAST_READ		0XEB


#define COMMAND_BULK_ERASE			0xC7 /* Bulk Erase command */
#define COMMAND_STATUSREG_READ		0x05 /* Status read command */

#define COMMAND_QUAD_IO_EN			0X35 /*ENTER QUAD INPUT/OUTPUT MODE */
#define COMMAND_QUAD_IO_DIS			0xF5


#define COMMAND_4BYTE_PAGE_PROGRAM	0x12 /* 4-BYTE PAGEPROGRAM */
#define COMMAND_4BYTE_READ			0x13 /* 4-BYTE READ */


#define COMMAND_4BYTE_FAST_READ		0x0c /* 4-BYTE FAST READ */
#define COMMAND_QUAD_READ			0x6B /* Quad Output Fast Read 0x6c*/

#define	COMMAND_WRITE_ENABLE		0x06 /* Write Enable command */
#define COMMAND_WRITE_DISABLE		0x04 /* WRITE DISABLE */
#define COMMAND_4BYTE_MODE_EN		0xB7
#define COMMAND_4BYTE_MODE_DIS		0xE9

#define COMMAND_PAGE_PROGRAM		0x02 /* Page Program command */

#define COMMAND_4BYTE_QUAD_PROGRAM	0x34 /* 4-BYTE QUAD INPUT FAST PROGRAM */

#define COMMAND_READ_STATUS			0x70 /* READ FLAG STATUS REGISTER */
#define COMMAND_SECTOR_ERASE		0xD8 /* Sector Erase command */


#define SPI_PROTO_STR       0
#define SPI_PROTO_DTR       1
#define SPI_PROTO_DUAL_STR  2
#define SPI_PROTO_DUAL_DTR  3
#define SPI_PROTO_QUAD_STR  4
#define SPI_PROTO_QUAD_DTR  5
/**
 * This definitions specify the EXTRA bytes in each of the command
 * transactions. This count includes Command byte, address bytes and any
 * don't care bytes needed.
 */
#define READ_WRITE_EXTRA_BYTES		5 /* Read/Write extra bytes */
#define	WRITE_ENABLE_BYTES		1 /* Write Enable bytes */
#define SECTOR_ERASE_BYTES		4 /* Sector erase extra bytes */
#define BULK_ERASE_BYTES		1 /* Bulk erase extra bytes */
#define STATUS_READ_BYTES		2 /* Status read bytes count */
#define STATUS_WRITE_BYTES		2 /* Status write bytes count */

/*
 * Flash not busy mask in the status register of the flash device.
 */
#define FLASH_SR_IS_READY_MASK		0x01 /* Ready mask */




/*
 * Byte Positions.
 */
#define BYTE1				0 /* Byte 1 position */
#define BYTE2				1 /* Byte 2 position */
#define BYTE3				2 /* Byte 3 position */
#define BYTE4				3 /* Byte 4 position */
#define BYTE5				4 /* Byte 5 position */
#define BYTE6				5 /* Byte 6 position */
#define BYTE7				6 /* Byte 7 position */
#define BYTE8				7 /* Byte 8 position */

/**************************** Type Definitions *******************************/


/***************** Macros (Inline Functions) Definitions *********************/
/*mt25qu02g*/
FlashInfo Flash_Config_Table = {
	0x20bb22, 
	0X10000U,		/**< 64K Sector */
	4096U,			/**< 4096 Sectors */
	256U,			/**< 256 Bytes per Page */
	0x100000, 		/*This is the size of one flash device */
	0x10000000, 	/* Mask to get sector start address */
	0xFFFF0000, 
	4							
};
/************************** Function Prototypes ******************************/
static int SpiFlashWriteEnable();
static int SpiFlashWriteDisable();
static int SpiFlashBulkErase();
static int SpiFlashSectorErase(size_t Addr,uint8_t cmd);
static int FlashEnterExit4BAddMode(int enable);
static int SpiFlashEnableHPM();
static int SpiFlashWaitForFlashReady(void);
static int ReadFlashID();
static int FlashWrite(size_t Addr, size_t ByteCount, u8 WriteCmd ,u8 *src);
static int FlashRead(size_t Addr, size_t ByteCount, u8 ReadCmd, u8 *dest);

/************************** Variable Definitions *****************************/

/*
 * The instances to support the device drivers are global such that they
 * are initialized to zero each time the program runs. They could be local
 * but should at least be static so they are zeroed.
 */
static XSpi Spi;

/*
 * Buffers used during read and write transactions.
 */
static u8 ReadBuffer[SECTOR_SIZE];
static u8 WriteBuffer[SECTOR_SIZE];

/**
 * @description: FLASH命令测试函数
 * @param {uint8_t} cmd
 * @param {int} readLen
 * @return {*}
 */
static int SpiFlashWriteCmd(uint8_t cmd,int readLen)
{
    int Status;
	Status = SpiFlashWriteEnable();
    if(Status != XST_SUCCESS) {
        return XST_FAILURE;
    }

    WriteBuffer[BYTE1] = cmd;
    /*
	 * Initiate the Transfer.
	 */
	Status = XSpi_Transfer(&Spi, WriteBuffer, ReadBuffer,
						(readLen+1));
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	Status = SpiFlashWriteDisable();
    if(Status != XST_SUCCESS) {
        return XST_FAILURE;
    }
	
	return ReadBuffer[1];
}
/************************** Function Definitions ******************************/


/**
 * @description: 块擦除函数
 * @return {*}
 */
static int SpiFlashBulkErase()
{
	int Status;

	/*
	 * Wait while the Flash is busy.
	 */
	Status = SpiFlashWaitForFlashReady();
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Prepare the WriteBuffer.
	 */
	WriteBuffer[BYTE1] = COMMAND_BULK_ERASE;

	/*
	 * Initiate the Transfer.
	 */
	Status = XSpi_Transfer(&Spi, WriteBuffer, NULL,
						BULK_ERASE_BYTES);
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}



	return XST_SUCCESS;
}


/**
 * @description: 擦除卷函数
 * @param {size_t} Addr
 * @param {uint8_t} cmd
 * @return {*}
 */
static int SpiFlashSectorErase(size_t Addr,uint8_t cmd)
{
	int Status;

	/*
	 * Wait while the Flash is busy.
	 */
	Status = SpiFlashWaitForFlashReady();
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Prepare the WriteBuffer.
	 */
	// WriteBuffer[BYTE1] = COMMAND_SECTOR_ERASE;
    WriteBuffer[BYTE1] = cmd;
	WriteBuffer[BYTE2] = (u8) (Addr >> 16);
	WriteBuffer[BYTE3] = (u8) (Addr >> 8);
	WriteBuffer[BYTE4] = (u8) (Addr);

	Status = XSpi_Transfer(&Spi, WriteBuffer, NULL,
					SECTOR_ERASE_BYTES);
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	return XST_SUCCESS;
}


/**
 * @description: 4字节地址模式配置
 * @param {int} enable
 * @return {*}
 */
static int FlashEnterExit4BAddMode(int enable)
{
	if (enable > 0) {
		SpiFlashWriteCmd(COMMAND_4BYTE_MODE_EN,0);
	} else {
		SpiFlashWriteCmd(COMMAND_4BYTE_MODE_DIS,0);
	}
	
	return XST_SUCCESS;
}

/**
 * @description: 开启FLASH性能模式
 * @return {*}
 */
static int SpiFlashEnableHPM()
{
	int Status;

	/*
	 * Perform the Write Enable operation.
	 */
	Status = SpiFlashWriteEnable(&Spi);
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Wait while the Flash is busy.
	 */
	Status = SpiFlashWaitForFlashReady();
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Prepare the WriteBuffer.
	 */
	WriteBuffer[BYTE1] = 0xA3;

	/*
	 * Initiate the Transfer.
	 */
	Status = XSpi_Transfer(&Spi, WriteBuffer, NULL, 4);
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}


	return XST_SUCCESS;
}

/**
 * @description: 判断FLASH是否准备好
 * @return {*}
 */
static int SpiFlashWaitForFlashReady(void)
{
	int Status;
	u8 StatusReg;

	while(1) {

		/*
		 * Get the Status Register.
		 */
		WriteBuffer[BYTE1] = COMMAND_STATUSREG_READ;


        Status = XSpi_Transfer(&Spi, WriteBuffer, ReadBuffer,
                            STATUS_READ_BYTES);
        if(Status != XST_SUCCESS) {
            return XST_FAILURE;
        }

		/*
		 * Check if the flash is ready to accept the next command.
		 * If so break.
		 */
		StatusReg = ReadBuffer[1];
		if((StatusReg & FLASH_SR_IS_READY_MASK) == 0) {
			break;
		}
		
        // sleep(1);
	}

	return XST_SUCCESS;
}


/**
 * @description: 获取指定MT25QU02 ID
 * @return {*}
 */
static int ReadFlashID()
{
	int Status,ReadId = 0;
	Status = SpiFlashWriteEnable();
    if(Status != XST_SUCCESS) {
        return XST_FAILURE;
    }

    WriteBuffer[BYTE1] = SPI_CMD_READ_ID;
    /*
	 * Initiate the Transfer.
	 */
	Status = XSpi_Transfer(&Spi, WriteBuffer, ReadBuffer,
						4);
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}
	ReadId = ((ReadBuffer[1] << 16) | (ReadBuffer[2] << 8) | ReadBuffer[3]);
	printf("Manufacturer ID: 0x%02x\n", ReadBuffer[1]);
    printf("Memory type: 0x%02x\n", ReadBuffer[2]);
    printf("Memory capacity: 0x%02x\n", ReadBuffer[3]);
	/*
	 * Assign corresponding index in the Flash configuration table
	 */
	if(Flash_Config_Table.jedec_id != ReadId) {
		return XST_FAILURE;
	}

	return XST_SUCCESS;
}

/**
 * @description: FLASH复位
 * @return {*}
 */
void SpiFlashReset()
{
    SpiFlashWriteCmd(SPI_CMD_RESET_ENABLE,1);
    SpiFlashWriteCmd(SPI_CMD_RESET_MEMORY,1);
}

/**
 * @description: 释放QSPI模块
 * @param {flash_device} *fdev
 * @return {*}
 */
void SpiFlashRelease(struct flash_device *fdev)
{
	FlashEnterExit4BAddMode(0);
	SpiFlashWriteDisable();
    XSpi_Stop(&Spi);
}

/**
 * @description: 外部调用QSPI模块初始化
 * @param {flash_device} *fdev
 * @return {*}
 */
int SpiFlashInit(struct flash_device *fdev)
{
	int Status;

	Status = XSpi_Initialize(&Spi, SPI_DEVICE_ID);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	Status = XSpi_SelfTest(&Spi);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	Status = XSpi_SetOptions(&Spi, XSP_MASTER_OPTION |
				 XSP_LOOPBACK_OPTION);
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	Status = XSpi_SetSlaveSelect(&Spi, SPI_SELECT);
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	XSpi_Start(&Spi);

    /*
	 * Disable Global interrupt to use polled mode operation
	 */
	XSpi_IntrGlobalDisable(&Spi);
	printf("Successfully flash quad Example\r\n");


    // SpiFlashReset();
    Status = ReadFlashID();
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}
	FlashEnterExit4BAddMode(1);

    fdev->size = ((size_t)Flash_Config_Table.FlashDeviceSize);

    printf("Flash size: %ld MB\n", fdev->size / (1 << 20));

    fdev->protocol = SPI_PROTO_STR;
    fdev->bulk_protocol = SPI_PROTO_STR;
    fdev->read_dummy_cycles = 0;
    fdev->write_buffer_size = PAGE_SIZE; //256字节
    fdev->erase_block_size = SUBSECTOR_SIZE; //每次擦除大小

    printf("Write buffer size: %ld B\n", fdev->write_buffer_size);
    printf("Erase block size: %ld B\n", fdev->erase_block_size);
	

	return XST_SUCCESS;
}

/**
 * @description: 写使能
 * @return {*}
 */
int SpiFlashWriteEnable()
{
	int Status;
	Status = SpiFlashWaitForFlashReady();
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}
	WriteBuffer[BYTE1] = SPI_CMD_WRITE_ENABLE;
	Status = XSpi_Transfer(&Spi, WriteBuffer, NULL,
				WRITE_ENABLE_BYTES);
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}
	return XST_SUCCESS;
}

/**
 * @description: 写失能
 * @return {*}
 */
int SpiFlashWriteDisable()
{
	int Status;

	WriteBuffer[BYTE1] = COMMAND_WRITE_DISABLE;

	Status = XSpi_Transfer(&Spi, WriteBuffer, NULL,
				WRITE_ENABLE_BYTES);
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}
	return XST_SUCCESS;
}

/**
 * @description: 内部测试写函数
 * @param {size_t} Addr
 * @param {size_t} ByteCount
 * @param {u8} WriteCmd
 * @param {u8} *src
 * @return {*}
 */
static int FlashWrite(size_t Addr, size_t ByteCount, u8 WriteCmd,u8 *src)
{
	int Status;

    uint8_t m_write[ByteCount + READ_WRITE_EXTRA_BYTES];
    memset(m_write,0,ByteCount + READ_WRITE_EXTRA_BYTES);

	Status = SpiFlashWriteEnable();
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}
	/*
	 * Wait while the Flash is busy.
	 */
	Status = SpiFlashWaitForFlashReady();
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Prepare the WriteBuffer.
	 */
	m_write[BYTE1] = WriteCmd;
	m_write[BYTE2] = (u8) (Addr >> 24);
	m_write[BYTE3] = (u8) (Addr >> 16);
	m_write[BYTE4] = (u8) (Addr >> 8);
	m_write[BYTE5] = (u8) Addr;


	memcpy(&m_write[READ_WRITE_EXTRA_BYTES],src,ByteCount);

	/*
	 * Initiate the Transfer.
	 */
	Status = XSpi_Transfer(&Spi, m_write, NULL,
				(ByteCount + READ_WRITE_EXTRA_BYTES));
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	Status = SpiFlashWriteDisable();
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}


	return XST_SUCCESS;
}

/**
 * @description: 内部测试读函数
 * @param {size_t} Addr
 * @param {size_t} ByteCount
 * @param {u8} ReadCmd
 * @param {u8} *dest
 * @return {*}
 */
static int FlashRead(size_t Addr, size_t ByteCount, u8 ReadCmd, u8 *dest)
{
	int Status;
	size_t len = (ByteCount > SECTOR_SIZE)?SECTOR_SIZE:ByteCount;
	/*
	 * Wait while the Flash is busy.
	 */
	Status = SpiFlashWaitForFlashReady();
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Prepare the WriteBuffer.
	 */
	WriteBuffer[BYTE1] = ReadCmd;
	WriteBuffer[BYTE2] = (u8) (Addr >> 24);
	WriteBuffer[BYTE3] = (u8) (Addr >> 16);
	WriteBuffer[BYTE4] = (u8) (Addr >> 8);
	WriteBuffer[BYTE5] = (u8) Addr;

	//暂时用不到
	if (ReadCmd == COMMAND_DUAL_READ) {
		len += 2;
	} else if (ReadCmd == COMMAND_DUAL_IO_READ) {
		len++;
	} else if (ReadCmd == COMMAND_QUAD_4BYTE_READ) {
		len += 3;
	} else if (ReadCmd==COMMAND_QUAD_READ) {
		len += 4;
	}

	/*
	 * Initiate the Transfer.
	 */
	Status = XSpi_Transfer( &Spi, WriteBuffer, ReadBuffer,
				(len + READ_WRITE_EXTRA_BYTES));
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}
	memcpy(dest,&ReadBuffer[READ_WRITE_EXTRA_BYTES],len);
	return XST_SUCCESS;
}



/**
 * @description: 外部调用FLASH操作函数
 * @param {flash_device} *fdev
 * @param {uint8_t} mode
 * @param {void} *msg
 * @return {*}
 */
int SpiFlashIoctl(struct flash_device *fdev, uint8_t mode, void *msg)
{
	struct flash_ioc *ioc = (struct flash_ioc *)msg;
	int ret = 0;
	switch (mode)
	{
	case 0:
		/* read */
		ret = FlashRead(ioc->addr,ioc->len,ioc->cmd,ioc->data);
		break;
	case 1:
		/* write */
		ret = FlashWrite(ioc->addr,ioc->len,ioc->cmd,ioc->data);
	default:
		break;
	}
	return ret;
}

/**
 * @description: 外部调用擦除函数
 * @param {flash_device} *fdev
 * @param {size_t} addr
 * @param {size_t} len
 * @return {*}
 */
int SpiFlashErase(struct flash_device *fdev, size_t addr, size_t len)
{
    int Status;
    size_t erase_block_size = fdev->erase_block_size;
    size_t eraseTatol_size = len;
    size_t erase_addr = addr;
    while (eraseTatol_size > 0)
    {
        // determine sector size
        erase_block_size = 0;

        if ((erase_addr & (SECTOR_SIZE-1)) == 0 && eraseTatol_size >= SECTOR_SIZE)
        {
            erase_block_size = SECTOR_SIZE;
        }
        else if ((erase_addr & (SUBSECTOR_SIZE-1)) == 0 && eraseTatol_size >= SUBSECTOR_SIZE)
        {
            erase_block_size = SUBSECTOR_SIZE;
        }

        // check size and alignment
        if (!erase_block_size)
        {
            fprintf(stderr, "Invalid erase request\n");
            return XST_FAILURE;
        }
        SpiFlashWriteEnable();
        /*
        * Wait while the Flash is busy.
        */
        Status = SpiFlashWaitForFlashReady();
        if(Status != XST_SUCCESS) {
            return XST_FAILURE;
        }

        // block erase
        if (fdev->size > 0x1000000)
        {
            if (erase_block_size == SECTOR_SIZE){
                WriteBuffer[BYTE1] = SPI_CMD_4B_SECTOR_ERASE;
            } else if (erase_block_size == SUBSECTOR_SIZE) {
                WriteBuffer[BYTE1] = SPI_CMD_4B_4KB_SUBSECTOR_ERASE;
            }
            WriteBuffer[BYTE2] = (u8) (erase_addr >> 24);
            WriteBuffer[BYTE3] = (u8) (erase_addr >> 16);
            WriteBuffer[BYTE4] = (u8) (erase_addr >> 8);
            WriteBuffer[BYTE5] = (u8) (erase_addr);
            Status = XSpi_Transfer(&Spi, WriteBuffer, NULL,
                        SECTOR_ERASE_BYTES+1);
            if(Status != XST_SUCCESS) {
                return XST_FAILURE;
            }
        } else {
            if (erase_block_size == SECTOR_SIZE) {
                SpiFlashSectorErase(erase_addr,SPI_CMD_SECTOR_ERASE);
            } else if (erase_block_size == SUBSECTOR_SIZE) {
                // normal 4KB subsector erase
                SpiFlashSectorErase(erase_addr,SPI_CMD_4KB_SUBSECTOR_ERASE);
            }
        }
        erase_addr += erase_block_size;
        eraseTatol_size -= erase_block_size;
    }
    return XST_SUCCESS;
}


/**
 * @description: 外部调用写函数
 * @param {flash_device} *fdev
 * @param {size_t} addr
 * @param {size_t} len
 * @param {void} *src
 * @return {*}
 */
int SpiFlashWrite(struct flash_device *fdev, size_t addr, size_t len, const void *src)
{
	u32 Index;
	int Status;
    const char *s = src;
	size_t m_addr = addr;
	int count = len / PAGE_SIZE;
	if ( len%PAGE_SIZE > 0 ) { //如果数据大于256
		count++;
	}
	Status = SpiFlashWriteEnable();
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}
	/*
	* Wait while the Flash is busy.
	*/
	Status = SpiFlashWaitForFlashReady();
	if(Status != XST_SUCCESS) {
		fprintf(stderr, "Failed to enable writing\n");
		return XST_FAILURE;
	}

	for(int page = 0; page < count; page++) { //循环写入

		if (fdev->size > 0x1000000)
		{
			// WriteBuffer[BYTE1] = COMMAND_4BYTE_PAGE_PROGRAM;
			WriteBuffer[BYTE1] = COMMAND_4BYTE_PAGE_PROGRAM;
			WriteBuffer[BYTE2] = (u8) (m_addr >> 24);
			WriteBuffer[BYTE3] = (u8) (m_addr >> 16);
			WriteBuffer[BYTE4] = (u8) (m_addr >> 8);
			WriteBuffer[BYTE5] = (u8) m_addr;
			Index = BYTE6;
		}
		else
		{
			WriteBuffer[BYTE1] = COMMAND_PAGE_PROGRAM;
			WriteBuffer[BYTE2] = (u8) (m_addr >> 16);
			WriteBuffer[BYTE3] = (u8) (m_addr >> 8);
			WriteBuffer[BYTE4] = (u8) m_addr;
			Index = BYTE5;
		}
		
		
		/*
		* Fill in the TEST data that is to be written into the Winbond Serial
		* Flash device.
		*/
	   	memcpy(&WriteBuffer[Index],s,PAGE_SIZE);

		/*
		* Initiate the Transfer.
		*/
		Status = XSpi_Transfer(&Spi, WriteBuffer, NULL,
					PAGE_SIZE + READ_WRITE_EXTRA_BYTES);
		if(Status != XST_SUCCESS) {
			return XST_FAILURE;
		}

		/*
		* Wait while the Flash is busy.
		*/
		Status = SpiFlashWaitForFlashReady();
		if(Status != XST_SUCCESS) {
			fprintf(stderr, "Failed to enable writing\n");
			return XST_FAILURE;
		}
		m_addr = m_addr + PAGE_SIZE;

	}

	Status = SpiFlashWriteDisable();
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}
	return XST_SUCCESS;
}


/**
 * @description: 外部调用读函数
 * @param {flash_device} *fdev
 * @param {size_t} addr
 * @param {size_t} len
 * @param {void} *dest
 * @return {*}
 */
int SpiFlashRead(struct flash_device *fdev, size_t addr, size_t len, void *dest)
{
	int Status;
    int Index = 0;
    char *d = dest;

    int protocol = SPI_PROTO_STR;

	if (fdev->size > 0x1000000) {
		// four byte address page program
		if (protocol == SPI_PROTO_QUAD_STR){
			/*
			* Prepare the WriteBuffer.
			*/
			WriteBuffer[BYTE1] = COMMAND_4BYTE_FAST_READ;
			
		} else {
			WriteBuffer[BYTE1] = COMMAND_4BYTE_READ;
		}
		WriteBuffer[BYTE2] = (u8) (addr >> 24);
		WriteBuffer[BYTE3] = (u8) (addr >> 16);
		WriteBuffer[BYTE4] = (u8) (addr >> 8);
		WriteBuffer[BYTE5] = (u8) addr;
		Index = BYTE6;
	} else {
		// normal page program
		if (protocol == SPI_PROTO_QUAD_STR){
			/*
			* Prepare the WriteBuffer.
			*/
			WriteBuffer[BYTE1] = COMMAND_FAST_READ;
			
		} else {
			WriteBuffer[BYTE1] = COMMAND_READ;
		}
		WriteBuffer[BYTE2] = (u8) (addr >> 16);
		WriteBuffer[BYTE3] = (u8) (addr >> 8);
		WriteBuffer[BYTE4] = (u8) addr;
		Index = BYTE5;
	}


	// if (WriteBuffer[BYTE1] == COMMAND_DUAL_READ) {
	// 	len += 2;
	// } else if (WriteBuffer[BYTE1] == COMMAND_DUAL_IO_READ) {
	// 	len++;
	// } else if (WriteBuffer[BYTE1] == COMMAND_QUAD_4BYTE_READ) {
	// 	len += 3;
	// } else if (WriteBuffer[BYTE1]==COMMAND_QUAD_READ) {
	// 	len += 4;
	// }

	/*
	* Initiate the Transfer.
	*/
	Status = XSpi_Transfer( &Spi, WriteBuffer, ReadBuffer,
				(Index + PAGE_SIZE));
	if(Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	* Wait while the Flash is busy.
	*/
	Status = SpiFlashWaitForFlashReady();
	if(Status != XST_SUCCESS) {
		fprintf(stderr, "Failed to enable writing\n");
		return XST_FAILURE;
	}
    
	memcpy(d,&ReadBuffer[Index],len);
	return XST_SUCCESS;
}




const struct flash_driver qspi_flash_driver = {
    .init = SpiFlashInit,
    .release = SpiFlashRelease,
    .read = SpiFlashRead,
    .write = SpiFlashWrite,
    .erase = SpiFlashErase,
	.ioctl = SpiFlashIoctl
};

